#include "uav_swarms/Follower.hpp"
// 订阅器回调函数，用于接收编队模式数据
void Follower::formation_pattern_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
    // 将数据转化为Eigen矩阵形式，并存储
    Eigen::VectorXf data = Eigen::Map<const Eigen::VectorXf>(msg->data.data(), msg->data.size());
    formation_pattern_ = Eigen::Map<const Eigen::MatrixXf>(data.data(), 3, uav_num);
    // std::cout << formation_pattern_ << std::endl;
    RCLCPP_INFO(this->get_logger(),"received values");
}

// 订阅器回调函数，更新无人机的当前位姿
void Follower::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
    pose_ = *msg;
}

// 订阅器回调函数，接收领队的位姿信息
void Follower::leader_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
    leader_pose_ = *msg;
}

// 订阅器回调函数，接收避障速度数据
void Follower::avoid_vel_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
    avoid_vel_ = *msg;
}

void Follower::leader_vel_callback(const  geometry_msgs::msg::TwistStamped::SharedPtr msg){
    std::lock_guard<std::mutex> lock_guard(leader_velocity_mutex);
    leader_velocity = *msg;
}

void Follower::follower_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg){
    std::lock_guard<std::mutex> lock_guard(follower_velocity_mutex);
    follower_velocity = *msg;
}

// 主控制循环，用于计算并发布速度指令
void Follower::control_loop() {

    // 等待一段时间，确保所有无人机都订阅了编队模式
    if (service_status.load() == false)
    {
        RCLCPP_INFO(this->get_logger(), "Waiting for formation pattern...");
        return;
    }
    

    // 只有在获取到编队模式后才进行控制
    if (formation_pattern_.size() != 0) {
        geometry_msgs::msg::Twist cmd_vel_enu;

        // 计算leader和follower之间的相对位置，并计算速度指令
        double p_diff_x = leader_pose_.pose.position.x + formation_pattern_(0, uav_id_)- pose_.pose.position.x;
        double p_diff_y = leader_pose_.pose.position.y + formation_pattern_(1, uav_id_)- pose_.pose.position.y;
        double p_diff_z = leader_pose_.pose.position.z + formation_pattern_(2, uav_id_)- pose_.pose.position.z;

        // double p_diff_x_v = p_diff_x / delta_t_ - follower_velocity.twist.linear.x;
        // double p_diff_y_v = p_diff_y / delta_t_ - follower_velocity.twist.linear.y;
        // double p_diff_z_v = p_diff_z / delta_t_ - follower_velocity.twist.linear.z;
        // 计算与领队的速度差异
        double v_diff_x = leader_velocity.twist.linear.x - follower_velocity.twist.linear.x;
        double v_diff_y = leader_velocity.twist.linear.y - follower_velocity.twist.linear.y;
        double v_diff_z = leader_velocity.twist.linear.z - follower_velocity.twist.linear.z;

        // 计算与领队的距离，并通过比例控制计算速度指令
        cmd_vel_enu.linear.x = p_diff_x * Kp_p * delta_t_ * diffusion_positon + v_diff_x * Kp_v * delta_t_ *diffusion_velocity;
        cmd_vel_enu.linear.y = p_diff_y * Kp_p * delta_t_* diffusion_positon + v_diff_y * Kp_v * delta_t_ *diffusion_velocity;
        cmd_vel_enu.linear.z = p_diff_z * Kp_p * delta_t_* diffusion_positon + v_diff_z * Kp_v * delta_t_ *diffusion_velocity;

        // cmd_vel_enu.linear.x = Kp_ * ((leader_pose_.pose.position.x + formation_pattern_(0, uav_id_)) - pose_.pose.position.x);
        // cmd_vel_enu.linear.y = Kp_ * ((leader_pose_.pose.position.y + formation_pattern_(1, uav_id_)) - pose_.pose.position.y);
        // cmd_vel_enu.linear.z = Kp_ * ((leader_pose_.pose.position.z + formation_pattern_(2, uav_id_)) - pose_.pose.position.z);
        // 添加避障速度
        cmd_vel_enu.linear.x += Kp_avoid_ * avoid_vel_.x * delta_t_ *diffusion_avoid;
        cmd_vel_enu.linear.y += Kp_avoid_ * avoid_vel_.y * delta_t_ *diffusion_avoid;
        cmd_vel_enu.linear.z += Kp_avoid_ * avoid_vel_.z * delta_t_ *diffusion_avoid;
        

        // 限制速度的最大值
        double cmd_vel_magnitude = sqrt(pow(cmd_vel_enu.linear.x, 2) + pow(cmd_vel_enu.linear.y, 2) + pow(cmd_vel_enu.linear.z, 2));
        if (cmd_vel_magnitude > sqrt(3) * vel_max_) {
            cmd_vel_enu.linear.x = cmd_vel_enu.linear.x / cmd_vel_magnitude * vel_max_;
            cmd_vel_enu.linear.y = cmd_vel_enu.linear.y / cmd_vel_magnitude * vel_max_;
            cmd_vel_enu.linear.z = cmd_vel_enu.linear.z / cmd_vel_magnitude * vel_max_;
        }

        // // 如果速度变化超过阈值，计算无人机的航向角（yaw），并进行角速度控制
        // if (fabs(cmd_vel_enu.linear.y) > 5.0 || fabs(cmd_vel_enu.linear.x) > 3.0) {
        //     double qx = pose_.pose.orientation.x;
        //     double qy = pose_.pose.orientation.y;
        //     double qz = pose_.pose.orientation.z;
        //     double qw = pose_.pose.orientation.w;

        //     // 计算领队的yaw角
        //     double yaw_leader = atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));
        //     cmd_vel_enu.angular.z = (atan2(cmd_vel_enu.linear.y, cmd_vel_enu.linear.x) - yaw_leader) * 4 * 0.05;
        // } else {
        //     cmd_vel_enu.angular.z = 0.0;
        // }

        // 发布速度指令
        vel_enu_pub_->publish(cmd_vel_enu);
        
        RCLCPP_INFO(this->get_logger(),"published twist is: %f, %f, %f", cmd_vel_enu.linear.x, cmd_vel_enu.linear.y, cmd_vel_enu.angular.z);
        // RCLCPP_INFO(this->get_logger(),"published twist is: %f, %f, %f", formation_pattern_(0, uav_id_), formation_pattern_(1, uav_id_), formation_pattern_(2, uav_id_));
    }
}

void Follower::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
    bool status = msg->status;
    service_status.store(status);
    RCLCPP_INFO(this->get_logger(), "Service status changed to %d", service_status.load());
}


void Follower::service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res){
    bool reqs = req->status;
    if(reqs){
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback.");
    }
    else{
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback false.");

    }
    service_status.store(reqs);
    res->success = true;
    res->drone_statue = "yolo detect service_status is " + std::to_string(service_status.load());
}